/*
 * Copyright (c) 2002,2016 Mario de Sousa (msousa@fe.up.pt)
 *
 * This file is part of the Modbus library for Beremiz and matiec.
 *
 * This Modbus library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.
 *
 * This code is made available on the understanding that it will not be
 * used in safety-critical situations without a full and competent review.
 */
#ifdef MODBUS_TCP
/************************************/
/**                                **/
/** Include common code...         **/
/**                                **/
/************************************/
#ifdef USE_LWIP

#include "lwip/def.h"
#include <lwip/sockets.h>

#endif

#ifdef ARDUINO

// #include <fcntl.h>
/* File control definitions 与lwip O_NONBLOCK有冲突*/

#elif defined(windows_x86)
#include <WinSock2.h>
#include <pthread.h>
typedef int socklen_t;
#define SHUT_RD 0x00
#define SHUT_WR 0x01
#define SHUT_RDWR 0x02
#define INET_ADDRSTRLEN 16
#define lwip_connect connect
#define lwip_accept accept
// #define sendmsg send
// #define lwip_read read
#define MSG_DONTWAIT 0x0
#if !defined(iovec)
struct iovec {
  void *iov_base;
  size_t iov_len;
};
#endif
struct msghdr {
  void *msg_name;
  socklen_t msg_namelen;
  struct iovec *msg_iov;
  int msg_iovlen;
  void *msg_control;
  socklen_t msg_controllen;
  int msg_flags;
};
#elif defined(LINUX)
// #include <termio.h> /* POSIX terminal control definitions */
#include <netinet/in.h>  /* required for htons() and ntohs() */
#include <netinet/ip.h>  /* IP  level socket options */
#include <netinet/tcp.h> /* TCP level socket options */
#include <pthread.h>
#include <sched.h> /* sched_yield() */
#include <sys/socket.h>
#include <unistd.h>

#include <fcntl.h>
#define lwip_connect connect
#define lwip_accept accept
// #define sendmsg send
#define lwip_read read
#define closesocket close
#elif defined(SYLIXOS)

#include <pthread.h>

#define closesocket close
extern "C" const char *inet_ntop(int af, const void *src, char *dst,
                                 socklen_t cnt);
#elif defined(MACOSX)
#define lwip_connect connect
#define lwip_accept accept
// #define sendmsg send
#define lwip_read read
#define closesocket close
#else
#include <netinet/in.h>  /* required for htons() and ntohs() */
#include <netinet/ip.h>  /* IP  level socket options */
#include <netinet/tcp.h> /* TCP level socket options */
#include <netinet/tcp.h>
#include <pthread.h>
#include <sched.h> /* sched_yield() */
#include <sys/socket.h>
#include <termio.h> /* POSIX terminal control definitions */

#endif
#ifdef __linux__
#include <netinet/tcp.h>
#endif

#include "mb_tcp_private.h"
#include "sin_util.h" /* internet socket utility functions... */

#undef bind

#include "Rtos.h"
#include "mb_layer1.h" /* The public interface this file implements... */
#include "mb_time_util.h"
#include <cerrno> /* Error definitions */
#include <cstdlib>
#include <cstring>
#include <unistd.h> /* POSIX Symbolic Constants */

// #include <sys/fcntl.h>

#ifdef MACOSX
#include <netinet/ip.h>
#include <netinet/tcp.h>

#endif
// #define ERRMSG
#define ERRMSG_HEAD "Modbus/TCP: "

#ifndef ERRMSG
#define ERRMSG
#endif
#ifdef ARDUINO
#if O_NONBLOCK != 1
#error "O_NONBLOCK error"
#endif
#endif
/**************************************************************/
/**************************************************************/
/****                                                      ****/
/****                                                      ****/
/****                Forward Declarations                  ****/
/****                    and Defaults                      ****/
/****                                                      ****/
/**************************************************************/
/**************************************************************/

/* A Node Descriptor metadata,
 *   Due to the fact that modbus TCP is connection oriented,
 *   and that if the client detects an error the connection
 *   must be shut down and re-established automatically,
 *   the modbus TCP layer needs to keep the address of the remote server.
 *
 * We do this by implementing a node descriptor table, in which each
 *   entry will have the remote address, and the file descriptor
 *   of the socket currently in use.
 *
 * We do not pass the file descriptor up to the next higher layer. We
 *   send them the node descriptor instead...
 */
#undef write
#undef read
#undef send
#define MB_MASTER_NODE 12
#define MB_LISTEN_NODE 14
#define MB_SLAVE_NODE 16
#define MB_FREE_NODE 18
using nd_type_t = sa_family_t;

using nd_entry_t = struct {
  int fd; /* socket descriptor == file descriptor */
  /* NOTE:
   *   Modbus TCP says that on error, we should close
   *   a connection and retry with a new connection.
   *   Since it takes time for a socket to close
   *   a connection if the remote server is down,
   *   we close the connection on the socket, close the
   *   socket itself, and create a new one for the new
   *   connection. There will be times when the node will
   *   not have any valid socket, and it will have to
   *   be created on the fly.
   *   When the node does not have a valid socket,
   *   fd will be set to -1
   */
  int node_type;           /*   What kind of use we are giving to this node...
                            *   If node_type == MB_MASTER_NODE
                            *      The node descriptor was initialised by the
                            *      modbus_connect() function.
                            *      The node descriptor is being used by a master
                            *      device, and the addr contains the address of the slave.
                            *      Remember that in this case fd may be >= 0 while
                            *      we have a valid connection, or it may be < 0 when
                            *      the connection needs to be reset.
                            *   If node_type == MB_LISTEN_NODE
                            *      The node descriptor was initialised by the
                            *      modbus_listen() function.
                            *      The node is merely used to accept() new connection
                            *      requests. The new slave connections will use another
                            *      node to transfer data.
                            *      In this case fd must be >= 0.
                            *      fd < 0 is an ilegal state and should never occur.
                            *   If node_type == MB_SLAVE_NODE
                            *      The node descriptor was initialised when a new
                            *      connection request arrived on a MB_LISTEN type node.
                            *      The node descriptor is being used by a slave device,
                            *      and is currently being used to connect to a master.
                            *      In this case fd must be >= 0.
                            *      fd < 0 is an ilegal state and should never occur.
                            *   If node_type == FREE_ND
                            *      The node descriptor is currently not being used.
                            *      In this case fd is set to -1, but is really irrelevant.
                            */
  struct sockaddr_in addr; /* The internet adress we are using.
                            *   If node_type == MB_MASTER_NODE
                            *      addr will be the address of the remote slave
                            *   If node_type == MB_LISTEN_NODE
                            *      addr will be the address of the local
                            * listening port and network interface If node_type
                            * == MB_SLAVE_NODE addr will be the address of the
                            * local port and network interface of the connection
                            * to the specific client.
                            */
  void *listen_node;       /* When a slave accepts a connection through a
                            * MB_LISTEN_NODE, it will       will use an empty node for
                            * the         new connection, and configure this new node
                            * to       use the         type MB_SLAVE_NODE.       The
                            * listen_node entry is       only used         by nodes of
                            * type        MB_SLAVE_NODE.       In this       case,
                            * listen_node       will be the node of type
                            * MB_LISTEN_NODE       through       which        the
                            * connection       request came       through...
                            */
  int close_on_silence;    /* A flag used only by Master Nodes.
                            * When (close_on_silence > 0), then the connection to
                            * the    slave device will be shut down whenever the
                            * modbus_tcp_silence_init() function is called.
                            * Remember that the connection will be automatically
                            * re-established the next time the user wishes to
                            * communicate    with the same slave (using this same
                            * node    descripto).    If the user wishes to comply
                            * with    the    sugestion    in the OpenModbus Spec,
                            * (s)he should    set    this flag    if a silence
                            * interval longer than 1    second    is expected.
                            */
  int print_connect_error; /* flag to guarantee we only print an error the first
                            * time we attempt to connect to a emote server.
                            * Stops us from generting a cascade of errors while
                            * the slave is down. Flag will get reset every time
                            * we successfully establish a connection, so a
                            * message is once again generated on the next error.
                            */
  u8 *recv_buf;            /* This node's receive buffer
                            * The library supports multiple simultaneous connections,
                            * and may need to receive multiple frames through mutiple nodes
                            * concurrently.            To make the library thread-safe, we
                            * use one            buffer for each node.
                            */
};

/* please make sure to lock the node table mutex before calling this function */
static int nd_entry_init(nd_entry_t *nde) {
  nde->addr.sin_family = AF_INET;
  nde->node_type = MB_FREE_NODE;
  nde->fd = -1; /* not currently connected... */
  /* initialise recv buffer */
  nde->recv_buf = (u8 *)malloc(sizeof(u8) * RECV_BUFFER_SIZE);
  if (nde->recv_buf == nullptr)
    return -1;
  return 0;
}

/* please make sure to lock the node table mutex before calling this function */
static int nd_entry_done(nd_entry_t *nde) {
  free(nde->recv_buf);
  return 0;
}

typedef struct {
  /* the array of node descriptors, and current size... */
  nd_entry_t *node;    /* array of node entries. if nullptr => node table not
                          initialized */
  int node_count;      /* total number of nodes in the node[] array */
  int free_node_count; /* number of free nodes in the node[] array */
  void *mutex;
} nd_table_t;

static int nd_table_done(nd_table_t *ndt) {
  if (ndt->node == nullptr)
    return 0;

  /* lock the mutex */
  while (Rtos::mutex_lock(ndt->mutex) != 0)
    Rtos::sched_yield();

  /* initialise the state of each node in the array... */
  for (int count = 0; count < ndt->node_count; count++) {
    nd_entry_done(&ndt->node[count]);
  } /* for() */

  free(ndt->node);
  Rtos::mutex_unlock(ndt->mutex);
  Rtos::mutex_destroy(ndt->mutex);
  *ndt = (nd_table_t){.node = nullptr, .node_count = 0, .free_node_count = 0};

  return 0;
}

#if 1

/* nd_table_init()
 *   Version 1 of the nd_table_init() function.
 *   If called more than once, 2nd and any subsequent calls will
 *   be interpreted as a request to confirm that it was already correctly
 *   initialized with the requested number of nodes.
 */
static int nd_table_init(nd_table_t *ndt, int nd_count) {
  if (ndt->node != nullptr) {
    /* this function has already been called, and the node table is already
     * initialised */
    return (ndt->node_count == nd_count) ? 0 : -1;
  }

  /* initialise the node table mutex... */
  ndt->mutex = Rtos::Create_Mutex();
  if (Rtos::mutex_lock(ndt->mutex) != 0) {

    perror("pthread_mutex_lock()");
    logger.error(
        " Unable to lock newly crated mutex while creating new node table!\n");

    Rtos::mutex_destroy(ndt->mutex);
    return -1;
  }

  /* initialise the node descriptor metadata array... */
  ndt->node = (nd_entry_t *)malloc(sizeof(nd_entry_t) * nd_count);
  if (ndt->node == nullptr) {

    logger.error(" Out of memory: error initializing node address buffer\n");
    Rtos::mutex_unlock(ndt->mutex);
    Rtos::mutex_destroy(ndt->mutex);
    return -1;
  }

  /* initialise the state of each node in the array... */
  for (int count = 0; count < nd_count; count++) {
    if (nd_entry_init(&ndt->node[count]) < 0) {
      Rtos::mutex_unlock(ndt->mutex);
      nd_table_done(ndt);
      return -1;
    }
    ndt->node_count = count + 1;
    ndt->free_node_count = count + 1;
  } /* for() */

  ndt->node_count = nd_count;
  ndt->free_node_count = nd_count;
  Rtos::mutex_unlock(ndt->mutex);
  return nd_count; /* number of succesfully created nodes! */
}

#else
/* nd_table_init()
 *   Version 2 of the nd_table_init() function.
 *   If called more than once, 2nd and any subsequent calls will
 *   be interpreted as a request to reserve an extra new_nd_count
 *   number of nodes. This will be done using realloc().
 */
static int nd_table_init(nd_table_t *ndt, int new_nd_count) {
  int count;

  if (ndt->node == nullptr) {
    /* Node table nt yet initialized => we must initialise the node table
     * mutex... */
    pthread_mutex_init(ndt->mutex, nullptr);
  }

  /* lock the mutex */
  while (Rtos::mutex_lock(ndt->mutex) != 0)
    Rtos::sched_yield();

  /* initialise the node descriptor metadata array... */
  ndt->node =
      realloc(ndt->node, sizeof(nd_entry_t) * (ndt->node_count + new_nd_count));
  if (ndt->node == nullptr) {
    logger.error(" Out of memory: error initializing node address buffer\n", );
    pthread_mutex_unlock(ndt->mutex);
    pthread_mutex_destroy(ndt->mutex);
    return -1;
  }

  /* initialise the state of each newly added node in the array... */
  for (count = ndt->node_count; count < ndt->node_count + new_nd_count;
       count++) {
    if (nd_entry_init(&ndt->node[count]) < 0) {
      pthread_mutex_unlock(ndt->mutex);
      return -1;
    }
  } /* for() */
  ndt->node_count += new_nd_count;
  ndt->free_node_count += new_nd_count;

  pthread_mutex_unlock(ndt->mutex);
  return new_nd_count; /* number of succesfully created nodes! */
}
#endif

static int nd_table_get_free_node(nd_table_t *ndt, nd_type_t nd_type) {
  while (Rtos::mutex_lock(ndt->mutex) != 0)
    Rtos::sched_yield();

  /* check for free nodes... */
  if (ndt->free_node_count <= 0) {
    /* no free nodes... */

    Rtos::mutex_unlock(ndt->mutex);
    return -1;
  }

  /* Decrement the free node counter...*/
  ndt->free_node_count--;

  /* search for a free node... */
  for (int count = 0; count < ndt->node_count; count++) {
    if (ndt->node[count].node_type == MB_FREE_NODE) {
      /* found one!! Allocate it to the new type! */
      ndt->node[count].node_type = nd_type;

      Rtos::mutex_unlock(ndt->mutex);

      return count;
    }
  } /* for() */

  /* Strange... We should have free nodes, but we didn't finda any! */
  /* Let's try to get into a consistent state, and return an error! */
  ndt->free_node_count = 0;

  Rtos::mutex_unlock(ndt->mutex);

  return -1;
}

static void nd_table_close_node(nd_table_t *ndt, nd_entry_t *nd) {

  /* lock the mutex */

  while (Rtos::mutex_lock(ndt->mutex) != 0)
    Rtos::sched_yield();

  if (nd->node_type == MB_FREE_NODE) {
    /* Node already free... */

    Rtos::mutex_unlock(ndt->mutex);
    return;
  }

  /* Increment the free node counter...*/
  ndt->free_node_count++;
  /* Mark the node as being free. */
  nd->node_type = MB_FREE_NODE;

  Rtos::mutex_unlock(ndt->mutex);
}

/**************************************************************/
/**************************************************************/
/****                                                      ****/
/****                                                      ****/
/****                Global Library State                  ****/
/****                                                      ****/
/****                                                      ****/
/**************************************************************/
/**************************************************************/

/* The node descriptor table... */
/* NOTE: The node_table_ Must be initialized correctly here! */
static nd_table_t nd_table_ = {
    .node = nullptr, .node_count = 0, .free_node_count = 0};

/**************************************************************/
/**************************************************************/
/****                                                      ****/
/****                                                      ****/
/****              Local Utility functions...              ****/
/****                                                      ****/
/****                                                      ****/
/**************************************************************/
/**************************************************************/

#define min(a, b) (((a) < (b)) ? (a) : (b))
#define max(a, b) (((a) > (b)) ? (a) : (b))

/************************************/
/**                                **/
/**  Configure socket for Modbus   **/
/**                                **/
/************************************/

static int configure_socket(int socket_id) {

  /* configure the socket */
  /* Set it to be non-blocking. This is safe because we always use select()
   * before reading from it! It is also required for the connect() call. The
   * default timeout in the TCP stack is much too long (typically blocks for 128
   * s ??) when the connect does not succedd imediately!
   */
#ifdef USE_LWIP
  if (lwip_fcntl(socket_id, F_SETFL, O_NONBLOCK) < 0) {
    logger.error(ERRMSG_HEAD
                 "Error configuring socket 'non-blocking' option. errno = %d\n",
                 errno);
  }
#elif defined(windows_x86)
#else
  if (fcntl(socket_id, F_SETFL, O_NONBLOCK) < 0) {
#ifdef ERRMSG
    perror("fcntl()");
    logger.error(ERRMSG_HEAD
                 "Error configuring socket 'non-blocking' option.\n");
#endif
  }
#endif
/* configure the socket */
/* set the TCP no delay flag. */
#if !defined(SYLIXOS)
  {
    int bool_opt = 1;
    if (setsockopt(socket_id, IPPROTO_TCP, TCP_NODELAY, (const char *)&bool_opt,
                   sizeof(bool_opt)) < 0) {
#ifdef ERRMSG
      perror("setsockopt()");
      logger.error(ERRMSG_HEAD
                   "Error configuring socket 'TCP no delay' option.\n");
#endif
    }
  }
#endif
  /* set the IP low delay option. */
  {
#ifndef windows_x86
    int priority_opt = IPTOS_LOWDELAY;
    if (setsockopt(socket_id, IPPROTO_IP, IP_TOS, (const void *)&priority_opt,
                   sizeof(priority_opt)) < 0) {
#ifdef ERRMSG
      perror("setsockopt()");
      logger.error(ERRMSG_HEAD
                   "Error configuring socket 'IP low delay' option.\n");
#endif
    }
#endif
  }

  /* send buffer */
  /* NOTE: For slave devices, that may be receiving multiple
   *       requests before they have a chance to reply to the first,
   *       it probably is a good idea to have a large receive buffer.
   *       So it is best to leave it with the default configuration, as it is
   *       larger than the largest Modbus TCP frame.
   *
   *       For the send buffer, a smaller buffer should suffice.
   *       However, it probably does not make sense to
   *       waste time asking for a smaller buffer, since the larger
   *       default buffer has already been allocated (the socket has already
   *       been created!)
   *
   *       We might just as well leave out the configuration of the socket
   *       buffer size...
   */
#define SOCK_BUF_SIZE 300 /* The size proposed in the Modbus TCP spec. */
  {
#ifdef SYLIXOS
    int gopt;
    int sopt = 2048;
    socklen_t len = sizeof(int);
    int ret = setsockopt(socket_id, SOL_SOCKET, SO_RCVBUF, &sopt, len);
    if (ret < 0) {
      fprintf(stderr, "setsockopt error.\n");
    }

    ret = getsockopt(socket_id, SOL_SOCKET, SO_RCVBUF, &gopt, &len);
    if (ret < 0) {
      fprintf(stderr, "getsocket error.\n");
    }
//        logger.debug("IPv4 recv buffer size: %d\n", gopt);
#elif !(defined(ARDUINO))
    int sock_buf_size;
    sock_buf_size = SOCK_BUF_SIZE;
    if (setsockopt(socket_id, SOL_SOCKET, SO_SNDBUF,
                   (const char *)&sock_buf_size, sizeof(sock_buf_size)) < 0){
        logger.error(ERRMSG_HEAD "Error configuring socket'send buffer' option.\n");
    }
//      return -1;
    /* recv buffer */
    sock_buf_size = SOCK_BUF_SIZE;
    if (setsockopt(socket_id, SOL_SOCKET, SO_RCVBUF,
                   (const char *)&sock_buf_size, sizeof(sock_buf_size)) < 0){
        logger.error(ERRMSG_HEAD "Error configuring socket'recv buffer' option.\n");
    }
#endif
  }
  return 0;
}

/************************************/
/**                                **/
/** Connect socket to remote host  **/
/**                                **/
/************************************/

/* This function will create a new socket, and connect it to a remote host... */
int open_connection(nd_entry_t *nd, uint32_t timeout) {
  int socket_id, con_res;

  //    logger.debug(" open_connection(): called, nd = %d\n", nd);

  if (nd->fd >= 0)
    /* nd already connected) */
    return nd->fd;

  if (nd->addr.sin_family != AF_INET)
    /* invalid remote address, or invalid nd */
    return -1;

  /* lets try to connect... */
  /* create the socket */
  if ((socket_id = socket(PF_INET, DEF_TYPE, 0 /* protocol_num */)) < 0) {
    perror("socket()");
    logger.error(" Error creating socket\n");
#ifdef ERRMSG
    perror("socket()");
    logger.error(ERRMSG_HEAD "Error creating socket\n");
#endif
    return -1;
  }

  /* configure the socket - includes setting non-blocking option! */
  if (configure_socket(socket_id) < 0) {
    closesocket(socket_id);
    return -1;
  }
#ifndef windows_x86
  char client_ipv4_str[INET_ADDRSTRLEN];
  inet_ntop(AF_INET, &nd->addr.sin_addr, client_ipv4_str, INET_ADDRSTRLEN);
  logger.debug("connect to %s:%d.\n", client_ipv4_str, nd->addr.sin_port);
#endif
  /* establish the connection to remote host */
  con_res =
      lwip_connect(socket_id, (struct sockaddr *)&(nd->addr), sizeof(nd->addr));

  /* The following condition is not strictly necessary
   * (we could let the code fall through)
   * but it does make the code easier to read/understand...
   */
  if (con_res >= 0)
    goto success_exit; /* connected succesfully on first try! */

  if (con_res < 0) {
    if ((errno != EINPROGRESS) && (errno != EALREADY))
      goto error_exit; /* error in connection request! */
    logger.error("lwip_connect error %d", errno);

    /* connection request is ongoing */
    /* EINPROGRESS -> first call to connect, EALREADY -> subsequent calls to
     * connect */
    /* Must wait for connect to complete at most 'timeout' seconds */
    {
      fd_set fdset;
      int res, so_error;
      socklen_t len;
      struct timespec end_time {
      }, *et_ptr;

      et_ptr = nullptr;
      if (timeout != 0) {
        et_ptr = &end_time;
        uint32_t t = timeout;
        end_time.tv_sec = t / 1000;
        end_time.tv_nsec = (t % 1000) * 1000000;
      }

      FD_ZERO(&fdset);
      FD_SET(socket_id, &fdset);

      res = my_select_tcp(socket_id + 1, nullptr, &fdset, et_ptr);
      if (res <= 0) {
        logger.error("my_select_tcp error %d", errno);
        goto error_exit; /* error on call to select */
      }

      len = sizeof(so_error);
      res =
          getsockopt(socket_id, SOL_SOCKET, SO_ERROR, (char *)&so_error, &len);
      if (res < 0) {
        logger.error("SOL_SOCKET, SO_ERROR error %d", errno);
        goto error_exit; /* error on call to getsockopt */
      }
      if (so_error != 0) {
        logger.error("so_error error %d", errno);
        goto error_exit; /* error on connection attempt */
      }
      goto success_exit; /* succesfully completed connection attempt! */
      /* goto sucess_exit is not strcitly necessary - we could let the code fall
       * through! */
    }
  }

success_exit:
  nd->fd = socket_id;
  /* Succesfully established connection => print a message next time we have
   * error. */
  nd->print_connect_error = 1;

  //    logger.debug(" open_connection(): returning...\n");
  return socket_id;

error_exit:
  logger.error("connect error %d", errno);
#ifdef ERRMSG
  if (nd->print_connect_error > 0) {
    perror("connect()");
    logger.error(ERRMSG_HEAD "Error establishing socket connection.\n");
    /* do not print more error messages for this node... */
    nd->print_connect_error = 0;
  }
#endif
  closesocket(socket_id);
  return -1;
}

/* This function will accept a new connection request, and attribute it to a new
 * node... */
static inline void *accept_connection(nd_entry_t *nd) {
  int socket_id;
  int new_nd;

  // logger.debug(" accept_connection(): called, nd = %d\n", nd);

  /* NOTE: We MUST accccept8) all connection requests, even if no new node is
   * available.
   *       => We first accept the connection request, and only later look for a
   * node. If no node is free/available for this new connections request, the
   *          connection will be accepted and immediately closed.
   *       Reason:
   *       When the library is used for a Modbus/TCP server and no free node is
   *        available, if we do not accept() all newly arrived connection
   * requests we would enter an infinite loop calling
   *           - select() (in modbus_tcp_read())
   *           - and accept_connection().
   *        Note that select() will continue to return immediately if the
   *        connection request is not accept()ted!
   */
  /* lets accept new connection request... */
  struct sockaddr_in client_addr {};
  memset(&client_addr, 0, sizeof(client_addr));
  socklen_t client_len = sizeof(client_addr);
  if ((socket_id =
           accept(nd->fd, (struct sockaddr *)&client_addr, &client_len)) < 0) {
#ifdef ERRMSG
    // perror("accept()");
    logger.error(
        ERRMSG_HEAD
        "Error while waiting for connection request from new client %d\n",
        socket_id);
#endif
    /* error establishing new connection... */
    return nullptr;
  }
#ifndef windows_x86
  char client_ipv4_str[INET_ADDRSTRLEN];
  inet_ntop(AF_INET, &client_addr.sin_addr, client_ipv4_str, INET_ADDRSTRLEN);

  //    logger.debug("Incoming connection from %s:%d.\n", client_ipv4_str,
  //    client_addr.sin_port);
  int optval = 1;
  setsockopt(socket_id, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(optval));
#endif

  /* find a free node */
  if ((new_nd = nd_table_get_free_node(&nd_table_, MB_SLAVE_NODE)) < 0) {
    /* no available free nodes for the new connection... */
    closesocket(socket_id);
    return nullptr;
  }

  /* configure the socket - includes setting the non-blocking option! */
  if (configure_socket(socket_id) < 0) {
    nd_table_close_node(
        &nd_table_,
        &nd_table_.node[new_nd]); /* first free up the un-used node. */
    closesocket(socket_id);
    return nullptr;
  }

  /* set up the node entry and update the fd sets */
  nd_table_.node[new_nd].fd = socket_id;
  nd_table_.node[new_nd].listen_node = nd;

  logger.debug("accept_connection(): returning new_nd = %d ,fd=%d\n", new_nd,
               socket_id);
  return &nd_table_.node[new_nd];
}

static inline void close_connection(nd_entry_t *nd) {
  if (nd->fd >= 0) {
    /* disconnect the tcp connection */
    shutdown(nd->fd, SHUT_RDWR);
#ifdef ERRMSG
    int res =
#endif
        closesocket(nd->fd);
#ifdef ERRMSG
    if (res < 0) {
      perror("close()");
      logger.error(ERRMSG_HEAD "Error closing socket\n");
    }
#endif
    nd->fd = -1;
  }

  if (nd->node_type == MB_SLAVE_NODE) {
    /* If it is a slave node, we will not be receiving any more data over this
     * disconnected node, (MB_SLAVE_NODE do not get re-connected!), so we free
     * the node...
     */
    nd_table_close_node(&nd_table_, nd);
  }
}

/************************************/
/**                                **/
/**     Data format conversion     **/
/**                                **/
/************************************/

/*
 * Functions to convert u16 variables
 * between network and host byte order
 *
 * NOTE: Modbus uses MSByte first, just like
 *       tcp/ip, so we use the htons() and
 *       ntoh() functions to guarantee
 *       code portability.
 */
static inline u16 mb_hton(u16 h_value) {
#if defined(windows_x86) || defined(LINUX) || defined(MACOSX)
  return htons(h_value);
#else
  return lwip_htons(h_value);
#endif
}

static inline u16 mb_ntoh(u16 m_value) {
#if defined(windows_x86) || defined(LINUX) || defined(MACOSX)
  return ntohs(m_value);
#else
  return lwip_ntohs(m_value);
#endif
}

static inline u8 msb(u16 value) {
  /*  return Most Significant Byte of value; */
  return (value >> 8) & 0xFF;
}

static inline u8 lsb(u16 value) {
  /*  return Least Significant Byte of value; */
  return value & 0xFF;
}

#define u16_v(char_ptr) (*((u16 *)(&(char_ptr))))

/************************************/
/**                                **/
/**   Build/Check a frame header   **/
/**                                **/
/************************************/

/* A modbus TCP frame header has 6 bytes...
 *   header[0-1] -> transaction id
 *   header[2-3] -> must be 0
 *   header[4-5] -> frame data length (must be <= 255)
 */
#if TCP_HEADER_LENGTH < 6
#error This code assumes a header size of 6 bytes, but TCP_HEADER_LENGTH < 6
#endif

static inline void build_header(u8 *header, u16 transaction_id,
                                u16 byte_count) {
  u16_v(header[0]) = mb_hton(transaction_id);
  header[2] = 0;
  header[3] = 0;
  u16_v(header[4]) = mb_hton(byte_count);
}

static inline int check_header(const u8 *header, u16 *transaction_id,
                               u16 *byte_count) {
  if ((header[2] != 0) || (header[3] != 0))
    return -1;

  *transaction_id = mb_ntoh(*(u16 *)(header + 0));
  *byte_count = mb_ntoh(*(u16 *)(header + 4));

  if (*byte_count > MAX_L2_FRAME_LENGTH)
    return -1;

  return 0;
}

/**************************************************************/
/**************************************************************/
/****                                                      ****/
/****                                                      ****/
/****              Sending of Modbus TCP Frames            ****/
/****                                                      ****/
/****                                                      ****/
/**************************************************************/
/**************************************************************/

// pthread_mutex_t sendmsg_mutex = PTHREAD_MUTEX_INITIALIZER;
#ifdef windows_x86
/* NOTE: this function MUST be thread safe!! */
int modbus_tcp_write(void *nd, /* node descriptor */
                     u8 *data, u16 data_length, u16 transaction_id,
                     uint32_t transmit_timeout, u8 crc_reverse) {
#define data_vector_size 2

  u8 header[TCP_HEADER_LENGTH+data_length];
  struct iovec data_vector[data_vector_size] = {
      {(void *)header, TCP_HEADER_LENGTH}, {nullptr, 0}};
  struct msghdr msg = {nullptr, 0, data_vector, data_vector_size, nullptr,
                       0,       0};

  int res, bytes_sent;
  auto *nd_entry = (nd_entry_t *)nd;

  //    logger.debug(" locking mutex...\n");
  //  while (pthread_mutex_lock(&sendmsg_mutex) != 0);

  /*************************
   * prepare the header...  *
   *************************/
  build_header(header, transaction_id, data_length);
  /* Print the hex value of each character that is about to be
   * sent over the bus.
   */
  //    {
  //        int i;
  //        logger.debug("modbus_tcp_write(): sending data...\n");
  //        for (i = 0; i < TCP_HEADER_LENGTH; i++)
  //            logger.debug("[0x%2X]", header[i]);
  //        for (i = 0; i < data_length; i++)
  //            logger.debug("[0x%2X]", data[i]);
  //        logger.debug("\n");
  //    }
  /******************************************
   * do we need to re-establish connection? *
   ******************************************/
  if (open_connection(nd_entry, transmit_timeout) < 0) {
    logger.error(
        "[%lu] modbus_tcp_write(): could not establish connection...\n",
        Rtos::pthread_self());
#ifdef ERRMSG
    logger.error(ERRMSG_HEAD "could not establish connection...\n");
#endif
    return -1;
  }

  /**********************
   * write to output... *
   **********************/
  /* TWO ALTERNATIVE IMPLEMENTATIONS !!! */

  /* We are optimising for the most likely case, and in doing that
   * we are making the least likely case have worse behaviour!
   * Read on for an explanation...
   *
   * - The optimised behaviour for the most likely case:
   * We have set the NO_DELAY flag on the socket, so the IP datagram
   * is not delayed and is therefore sent as soon as any data is written to
   * the socket.
   * In order to send the whole message in a single IP datagram, we have to
   * write both the the header and the data with a single call to write()
   * In order to not to have to copy the data around just to add the
   * message header, we use sendmsg() instead of write()!
   *
   * - The worse behaviour for the least likely case:
   * If for some reason only part of the data is sent with the first call to
   * write(), a datagram is sent right away, and the subsequent data will
   * be sent in another datagram. :-(
   */
  /* NOTE: since snedmsg() is not thread safe, we use a mutex to protect access
   * to this function... */
  data_vector[data_vector_size - 1].iov_base = data;
  data_vector[data_vector_size - 1].iov_len = data_length;
  data_vector[0].iov_base = header;
  data_vector[0].iov_len = TCP_HEADER_LENGTH;
  memcpy(&header[TCP_HEADER_LENGTH], data,data_length);
  bytes_sent = 0;
  while (true) {
    int sendmsg_errno;
    /* Please see the comment just above the main loop!! */
    res = send(nd_entry->fd,(const char*) header, data_length + TCP_HEADER_LENGTH, 0);
    sendmsg_errno = errno;
    if (res < 0) {
      if ((sendmsg_errno != EAGAIN) && (sendmsg_errno != EINTR)) {
        /* error sending message... */
        close_connection(nd_entry);
        return -1;
      } else {
        continue;
      }
    } else {
      /* res >= 0 */
      bytes_sent += res;
      if (bytes_sent >= data_length + TCP_HEADER_LENGTH) {
        /* query succesfully sent! */
        // logger.debug("[%lu] modbus_tcp_write(): sent %d bytes ,fd=%d\n",
        //              Rtos::pthread_self(), bytes_sent,nd_entry->fd);
        //        pthread_mutex_unlock(&sendmsg_mutex);
        //        logger.debug("[%lu] unlocked  mutex...\n", pthread_self());
        return data_length;
      }

      /* adjust the data_vector... */

      if (res < data_vector[0].iov_len) {
        auto tmp = (u8 *)data_vector[0].iov_base;
        tmp += res;
        data_vector[0].iov_len -= res;
        data_vector[0].iov_base = tmp;
      } else {
        auto tmp = (u8 *)data_vector[1].iov_base;
        tmp += res;
        res -= data_vector[0].iov_len;
        data_vector[0].iov_len = 0;
        data_vector[1].iov_len -= res;
        data_vector[1].iov_base = tmp;
      }
    }
  } /* while (1) */
}
#else
int modbus_tcp_write(void *nd, /* node descriptor */
                     u8 *data, u16 data_length, u16 transaction_id,
                     uint32_t transmit_timeout, u8 crc_reverse) {
#define data_vector_size 2

  u8 header[TCP_HEADER_LENGTH];
  struct iovec data_vector[data_vector_size] = {
      {(void *)header, TCP_HEADER_LENGTH}, {nullptr, 0}};
  struct msghdr msg = {nullptr, 0, data_vector, data_vector_size, nullptr,
                       0,       0};

  int res, bytes_sent;
  auto *nd_entry = (nd_entry_t *)nd;

  //    logger.debug(" locking mutex...\n");
  //  while (pthread_mutex_lock(&sendmsg_mutex) != 0);

  /*************************
   * prepare the header...  *
   *************************/
  build_header(header, transaction_id, data_length);
  /* Print the hex value of each character that is about to be
   * sent over the bus.
   */
  //    {
  //        int i;
  //        logger.debug("modbus_tcp_write(): sending data...\n");
  //        for (i = 0; i < TCP_HEADER_LENGTH; i++)
  //            logger.debug("[0x%2X]", header[i]);
  //        for (i = 0; i < data_length; i++)
  //            logger.debug("[0x%2X]", data[i]);
  //        logger.debug("\n");
  //    }
  /******************************************
   * do we need to re-establish connection? *
   ******************************************/
  if (open_connection(nd_entry, transmit_timeout) < 0) {
    logger.error(
        "[%lu] modbus_tcp_write(): could not establish connection...\n",
        Rtos::pthread_self());
#ifdef ERRMSG
    logger.error(ERRMSG_HEAD "could not establish connection...\n");
#endif
    return -1;
  }

  /**********************
   * write to output... *
   **********************/
  /* TWO ALTERNATIVE IMPLEMENTATIONS !!! */

  /* We are optimising for the most likely case, and in doing that
   * we are making the least likely case have worse behaviour!
   * Read on for an explanation...
   *
   * - The optimised behaviour for the most likely case:
   * We have set the NO_DELAY flag on the socket, so the IP datagram
   * is not delayed and is therefore sent as soon as any data is written to
   * the socket.
   * In order to send the whole message in a single IP datagram, we have to
   * write both the the header and the data with a single call to write()
   * In order to not to have to copy the data around just to add the
   * message header, we use sendmsg() instead of write()!
   *
   * - The worse behaviour for the least likely case:
   * If for some reason only part of the data is sent with the first call to
   * write(), a datagram is sent right away, and the subsequent data will
   * be sent in another datagram. :-(
   */
  /* NOTE: since snedmsg() is not thread safe, we use a mutex to protect access
   * to this function... */
  data_vector[data_vector_size - 1].iov_base = data;
  data_vector[data_vector_size - 1].iov_len = data_length;
  data_vector[0].iov_base = header;
  data_vector[0].iov_len = TCP_HEADER_LENGTH;
  bytes_sent = 0;
  while (true) {
    int sendmsg_errno;
    /* Please see the comment just above the main loop!! */
    res = sendmsg(nd_entry->fd, &msg, 0);
    sendmsg_errno = errno;
    if (res < 0) {
      if ((sendmsg_errno != EAGAIN) && (sendmsg_errno != EINTR)) {
        /* error sending message... */
        close_connection(nd_entry);
        return -1;
      } else {
        continue;
      }
    } else {
      /* res >= 0 */
      bytes_sent += res;
      if (bytes_sent >= data_length + TCP_HEADER_LENGTH) {
        /* query succesfully sent! */
        // logger.debug("[%lu] modbus_tcp_write(): sent %d bytes ,fd=%d\n",
        //              Rtos::pthread_self(), bytes_sent,nd_entry->fd);
        //        pthread_mutex_unlock(&sendmsg_mutex);
        //        logger.debug("[%lu] unlocked  mutex...\n", pthread_self());
        return data_length;
      }

      /* adjust the data_vector... */

      if (res < data_vector[0].iov_len) {
        auto tmp = (u8 *)data_vector[0].iov_base;
        tmp += res;
        data_vector[0].iov_len -= res;
        data_vector[0].iov_base = tmp;
      } else {
        auto tmp = (u8 *)data_vector[1].iov_base;
        tmp += res;
        res -= data_vector[0].iov_len;
        data_vector[0].iov_len = 0;
        data_vector[1].iov_len -= res;
        data_vector[1].iov_base = tmp;
      }
    }
  } /* while (1) */
}
#endif
/**************************************************************/
/**************************************************************/
/****                                                      ****/
/****                                                      ****/
/****              Receiving Modbus TCP Frames             ****/
/****                                                      ****/
/****                                                      ****/
/**************************************************************/
/**************************************************************/

/* A helper function to modbus_tcp_read()
 *
 * WARNING: The semantics of this function are not what you would expect!
 *
 *          if (data_already_available != 0)
 *          It assumes that select() has already been called before
 *          this function got called, and we are therefore guaranteed
 *          to have at least one byte to read off the socket (the fd).
 *
 *          if (data_already_available == 0)
 *          it starts off by calling select()!
 *
 *
 * NOTE: Ususal select semantics for (a: end_time == nullptr) and
 *       (b: *end_time == 0) also apply.
 *
 *       (a) Indefinite timeout
 *       (b) Try once, and and quit if no data available.
 */
/* RETURNS: number of bytes read
 *          -1 read error!
 *          -2 timeout
 */
int tcp_read_bytes(int fd, u8 *data, int max_data_count,
                   struct timespec *end_time, int data_already_available) {
  fd_set rfds;
  int res, data_count;

  data_count = 0;

  while (data_count < max_data_count) {
    /*============================*
     * wait for data availability *
     *============================*/
    if (data_already_available == 0) {
      int sel_res;
      FD_ZERO(&rfds);
      FD_SET(fd, &rfds);
      sel_res = my_select_tcp(fd + 1, &rfds, nullptr, end_time);
      if (sel_res < 0)
        return -1;
      if (sel_res == 0)
        /* timeout! */
        return -2;
    }

    /*============================*
     * read the available data... *
     *============================*/
    logger.debug("lwip_read : fd=%d,data=0x%x,max_data_count=%d data_count=%d",
                 fd, data, max_data_count, data_count);
    res = recv(fd, (char *)data + data_count, max_data_count - data_count,
               MSG_DONTWAIT);
    if (res == 0) {
      /* We are guaranteed to have data to read off the fd since we called
       * select(), but read() returned 0 bytes.
       * This means that the remote process has closed down the connection,
       * so we return 0.
       */
      return 0;
    }

    if (res < 0) {
      logger.error("lwip_read error %d (%s),res=%d", errno, strerror(errno),
                   res);
      if (errno != EINTR)
        return -1;
      else
        res = 0;
    }
    data_count += res;
    data_already_available = 0;
  } /* while ()*/

  /* data read succesfully... */
  return data_count;
}

/***************************************/
/**                                   **/
/**    Read a Modbus TCP frame        **/
/**    off a single identified node.  **/
/**                                   **/
/***************************************/

/* This private function will read a Modbus TCP frame off a single identified
 * node that we know before hand that has data ready to be read off it. The data
 * may or may not be a valid Modbus TCP frame. It is up to this function to
 * figure that out.
 */
/* NOTES:
 *  - We re-use the recv_buf_ to load the frame header, so we have to make
 *    sure that the buffer is large enough to take it...
 */
/* RETURNS: number of bytes read
 *          -1 on read from file/node error
 *          -2 on timeout
 */
#if RECV_BUFFER_SIZE < TCP_HEADER_LENGTH
#error The receive buffer is smaller than the frame header length.
#endif

int modbus_tcp_read_frame(int nd, u16 *transaction_id,
                          struct timespec *ts_ptr) {
  int fd, res;
  u16 frame_length;

  //    logger.debug("[%lu] modbus_tcp_read_frame(): reading off nd=%d\n",
  //    Rtos::pthread_self(), nd);

  /*=========================*
   * read a Modbus TCP frame *
   *=========================*/
  /* assume error... */
  fd = nd_table_.node[nd].fd;

  /*-------------*
   * read header *
   *-------------*/
  if ((res = tcp_read_bytes(fd, nd_table_.node[nd].recv_buf, TCP_HEADER_LENGTH,
                            ts_ptr, 1)) != TCP_HEADER_LENGTH) {

    //        logger.debug("[fd:%d] modbus_tcp_read_frame(): frame with
    //        insuficient bytes for a valid header...\n", fd);

    if (res < 0)
      return res;
    return -1;
  }

  /* let's check for header consistency... */
  if (check_header(nd_table_.node[nd].recv_buf, transaction_id, &frame_length) <
      0) {

    //        logger.debug("[%lu] modbus_tcp_read_frame(): frame with non valid
    //        header...\n", Rtos::pthread_self());

    return -1;
  }

  /*-----------*
   * read data *
   *-----------*/
  if ((res = tcp_read_bytes(fd, nd_table_.node[nd].recv_buf, frame_length,
                            ts_ptr, 0)) != frame_length) {

    //        logger.debug("[%lu] modbus_tcp_read_frame(): frame with non valid
    //        frame length...\n", Rtos::pthread_self());
    if (res < 0)
      return res;
    return -1;
  }

  /* frame received succesfully... */

  return frame_length;
}

/***************************************/
/**                                   **/
/**    Read a Modbus TCP frame        **/
/**    OR Accept connection requests  **/
/**    off possibly multiple node...  **/
/**                                   **/
/***************************************/

/* The public function that reads a valid modbus frame.
 * The frame is read from...:
 *   -  if (nd >= 0) and (nd is of type MB_MASTER_NODE or MB_SLAVE_NODE)
 *          The frame is read from the node descriptor nd
 *   -  if (nd >= 0) and (nd is of type MB_LISTEN_NODE)
 *          The frame is read from the all node descriptors of type
 * MB_SLAVE_NODE that were opened as a consequence of a connection request to
 * the nd slave. In this case, new connection requests to nd will also be
 * accepted!
 *   -  if (nd == -1)
 *          The frame is read from any valid and initialised node descriptor.
 *          In this case, new connection requests to any nd of type
 * MB_LISTEN_NODE will also be accepted! In this case, the node where the data
 * is eventually read from is returned in *nd.
 *
 * The send_data and send_length parameters are ignored...
 *  (However, these parameters must stay in order to keep the function
 *   interface identical to the ASCII and RTU versons!)
 *
 * return value: The length (in bytes) of the valid frame,
 *               -1 on error
 *
 * NOTE: Ususal select semantics for (a: recv_timeout == nullptr) and
 *       (b: *recv_timeout == 0) also apply.
 *
 *       (a) Indefinite timeout
 *       (b) Try once, and and quit if no data available.
 */

/* RETURNS: number of bytes read
 *          -1 on read from file/node error
 *          -2 on timeout
 */
int modbus_tcp_read(void **nd, /* node descriptor */
                    u8 **recv_data_ptr, u16 *transaction_id,
                    const u8 *, /* ignored ! */
                    u16,        /* ignored ! */
                    uint32_t recv_timeout, u8 crc_reverse) {
  auto nd_entry = (nd_entry_t **)nd;
  struct timespec end_time {
  }, *ts_ptr;
  u8 *local_recv_data_ptr;
  u16 local_transaction_id = 0;

  // logger.debug("[%lu] modbus_tcp_read(): called...  nd=%d\n",
  // Rtos::pthread_self(), *nd);

  if (nd == nullptr)
    return -1;

  if (recv_data_ptr == nullptr)
    recv_data_ptr = &local_recv_data_ptr;
  if (transaction_id == nullptr)
    transaction_id = &local_transaction_id;

  /* We will potentially call read() multiple times to read in a single frame.
   * We therefore determine the absolute time_out, and use this as a parameter
   * for each call to tcp_read_bytes() instead of using a relative timeout.
   *
   * NOTE: see also the timeout related comment in the tcp_read_bytes()
   * function!
   */
  ts_ptr = nullptr;
  if (recv_timeout > 0) {
    ts_ptr = &end_time;
    end_time.tv_sec = recv_timeout / 1000;
    end_time.tv_nsec = (recv_timeout % 1000) * 1000000;
  }

  /* If we must read off a single node... */
  if ((((*nd_entry)->node_type == MB_FREE_NODE) || ((*nd_entry)->fd < 0)))
    /* but the node does not have a valid fd */
    return -1;

  /* We will loop forever...
   * We jump out of the loop and return from the function as soon as:
   *  - we receive a valid modbus message;
   *    OR
   *  - we time out.
   *
   *  NOTE: This loop will close connections through which we receive invalid
   * frames. This means that the set of nodes through which we may receive data
   * may change with each loop iteration.  => We need to re-calculate the fds in
   * each loop iteration!
   */

  while (true) {
    int nd_count;
    int fd_high;
    fd_set rfds;

    /* We prepare our fd sets here so we can later call select() */
    FD_ZERO(&rfds);
    fd_high = -1;

    for (nd_count = 0; nd_count < nd_table_.node_count; nd_count++) {
      if (nd_table_.node[nd_count].node_type != MB_FREE_NODE &&
          ((*nd_entry) == &nd_table_.node[nd_count] ||
           (nd_table_.node[nd_count].node_type == MB_SLAVE_NODE) &&
               (nd_table_.node[nd_count].listen_node == (*nd_entry))) &&
          nd_table_.node[nd_count].fd >= 0) {
        /* Add the descriptor to the fd set... */
        FD_SET(nd_table_.node[nd_count].fd, &rfds);
        fd_high = max(fd_high, nd_table_.node[nd_count].fd);
      }
    } /* for(;;) */

    // logger.debug("[%lu] modbus_tcp_read(): while(1) looping. fd_high = %d,
    // nd=%d\n", Rtos::pthread_self(), fd_high,                     *nd);

    if (fd_high == -1)
      /* we will not be reading from any node! */
      return -1;

    /* We now call select and wait for activity on the nodes we are listening to
     */
    {
      int sel_res = my_select_tcp(fd_high + 1, &rfds, nullptr, ts_ptr);
      if (sel_res < 0)
        return -1;
      if (sel_res == 0)
        /* timeout! */
        return -2;
    }

    /* figure out which nd is ready to be read... */
    for (nd_count = 0; nd_count < nd_table_.node_count; nd_count++) {
      if ((nd_table_.node[nd_count].node_type != MB_FREE_NODE) &&
          (nd_table_.node[nd_count].fd >= 0) &&
          FD_ISSET(nd_table_.node[nd_count].fd, &rfds)) {
        /* Found the node descriptor... */

        // logger.debug("[%lu] modbus_tcp_read(): my_select() returned due to
        // activity on node nd=%d\n",
        //              Rtos::pthread_self(), nd_count);

        if (nd_table_.node[nd_count].node_type == MB_LISTEN_NODE) {
          if (accept_connection(&nd_table_.node[nd_count]) == nullptr) {
            logger.error(
                "[%lu] modbus_tcp_read(): accept_connection() failed!\n",
                Rtos::pthread_self());
            return -1;
          }
        } else {
          /* it is a MB_SLAVE_NODE or a MB_MASTER_NODE */
          /* We will read a frame off this nd */
          int res;
          res = modbus_tcp_read_frame(nd_count, transaction_id, ts_ptr);
          if (res > 0) {
            *nd_entry = &nd_table_.node[nd_count];
            *recv_data_ptr = nd_table_.node[nd_count].recv_buf;
            return res;
          }
          if (res < 0) {
            /* We had an error reading the frame...
             * We handle it by closing the connection, as specified by
             * the modbus TCP protocol!
             *
             * NOTE: The error may have been a timeout, which means this
             * function should return immediately. However, in this case we let
             * the execution loop once again in the while(1) loop. My_select()
             * will be called again and the timeout detected. The timeout error
             * code (-2) will then be returned correctly!
             */

            logger.error("[%lu] modbus_tcp_read(): error reading frame. "
                         "Closing connection...\n",
                         Rtos::pthread_self());

            /* We close the socket... */
            close_connection(&nd_table_.node[nd_count]);
          }
        }
        /* we have found the node descriptor, so let's jump out of the for(;;)
         * loop */
        break;
      }
    } /* for(;;) */

    /* We were unsuccesfull reading a frame, so we try again... */
  } /* while (1) */
}

/**************************************************************/
/**************************************************************/
/****                                                      ****/
/****                                                      ****/
/****        Initialising and Shutting Down Library        ****/
/****                                                      ****/
/****                                                      ****/
/**************************************************************/
/**************************************************************/

/* Ugly hack...
 *  Beremiz will be calling modbus_tcp_init() multiple times (through
 * modbus_init() ) (once for each plugin instance) It will also be calling
 * modbus_tcp_done() the same number of times We only want to really shutdown
 * the library the last time it is called. We therefore keep a counter of how
 * many times modbus_tcp_init() is called, and decrement it in modbus_tcp_done()
 */
int modbus_tcp_init_counter = 0;

/******************************/
/**                          **/
/**   Load Default Values    **/
/**                          **/
/******************************/

static void set_defaults(const char **service) {
  /* Set the default values, if required... */
  if (*service == nullptr)
    *service = DEF_SERVICE;
}

/******************************/
/**                          **/
/**    Initialise Library    **/
/**                          **/
/******************************/
/* returns the number of nodes succesfully initialised...
 * returns -1 on error.
 */
int modbus_tcp_init(int nd_count, optimization_t opt /* ignored... */,
                    int *extra_bytes) {
  //    logger.debug("[%lu] modbus_tcp_init(): called...\n",
  //    Rtos::pthread_self());
  logger.debug("[%lu] creating %d nodes:\n", Rtos::pthread_self(), nd_count);

  modbus_tcp_init_counter++;

  /* set the extra_bytes value... */
  /* Please see note before the modbus_rtu_write() function for a
   * better understanding of this extremely ugly hack... This will be
   * in the mb_rtu.c file!!
   *
   * The number of extra bytes that must be allocated to the data buffer
   * before calling modbus_tcp_write()
   */
  if (extra_bytes != nullptr)
    *extra_bytes = 0;

  if (0 == nd_count)
    /* no need to initialise this layer! */
    return 0;
  if (nd_count <= 0)
    /* invalid node count... */
    goto error_exit_1;

  /* initialise the node table... */
  if (nd_table_init(&nd_table_, nd_count) < 0)
    goto error_exit_1;

  logger.debug("[%lu] modbus_tcp_init(): %d node(s) opened succesfully\n",
               Rtos::pthread_self(), nd_count);

  return nd_count; /* number of succesfully created nodes! */

  /*
  error_exit_2:
    nd_table_done(&nd_table_);
  */
error_exit_1:
  if (extra_bytes != nullptr)
    *extra_bytes = 0;
  return -1;
}

/******************************/
/**                          **/
/**    Open a Master Node    **/
/**                          **/
/******************************/
void *modbus_tcp_connect(node_addr_t *node_addr) {
  int node_descriptor;
  struct sockaddr_in tmp_addr {};

  //    logger.debug("[%lu] modbus_tcp_connect(): called...\n",
  //    Rtos::pthread_self());
  logger.debug("[%lu]        0x%x:%s\n", Rtos::pthread_self(),
               node_addr->addr.tcp.host, node_addr->addr.tcp.service);

  /* Check for valid address family */
  if (node_addr->naf != naf_tcp)
    /* wrong address type... */
    return nullptr;

  /* set the default values... */
  set_defaults(&(node_addr->addr.tcp.service));

  /* Check the parameters we were passed... */
  if (sin_initaddr(&tmp_addr, node_addr->addr.tcp.host, 1,
                   node_addr->addr.tcp.service, 1, DEF_PROTOCOL) < 0) {
#ifdef ERRMSG
    logger.error(ERRMSG_HEAD "Error parsing/resolving address %s:%s\n",
                 node_addr->addr.tcp.host, node_addr->addr.tcp.service);
#endif
    return nullptr;
  }

  /* find a free node descriptor */
  if ((node_descriptor = nd_table_get_free_node(&nd_table_, MB_MASTER_NODE)) <
      0)
    /* if no free nodes to initialize, then we are finished... */
    return nullptr;

  nd_table_.node[node_descriptor].addr = tmp_addr;
  nd_table_.node[node_descriptor].fd = -1; /* not currently connected... */
  nd_table_.node[node_descriptor].close_on_silence =
      node_addr->addr.tcp.close_on_silence;
  nd_table_.node[node_descriptor].listen_node = nullptr;
  if (nd_table_.node[node_descriptor].close_on_silence < 0)
    nd_table_.node[node_descriptor].close_on_silence = DEF_CLOSE_ON_SILENCE;

  /* WE have never tried to connect, so print an error the next time we try to
   * connect */
  nd_table_.node[node_descriptor].print_connect_error = 1;

  //    logger.debug("[%lu] modbus_tcp_connect(): returning nd=%d\n",
  //    Rtos::pthread_self(), node_descriptor);

  return &nd_table_.node[node_descriptor];
}

/******************************/
/**                          **/
/**    Open a Slave Node     **/
/**                          **/
/******************************/

void *modbus_tcp_listen(node_addr_t *node_addr) {
  int fd, nd;

  //    logger.debug("[%lu] modbus_tcp_listen(): called...\n",
  //    Rtos::pthread_self());
  logger.debug("[%lu]        %s:%s\n", Rtos::pthread_self(),
               node_addr->addr.tcp.host, node_addr->addr.tcp.service);

  /* Check for valid address family */
  if (node_addr->naf != naf_tcp)
    /* wrong address type... */
    goto error_exit_0;

  /* set the default values... */
  set_defaults(&(node_addr->addr.tcp.service));
#ifdef windows_x86
  WSADATA wsaData;

  int iResult;

  iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
  if (iResult != 0) {
    logger.error("WSAStartup failed: %d\n", iResult);
    return nullptr;
  }
#endif
  /* create a socket and bind it to the appropriate port... */

  fd = sin_bindsock(node_addr->addr.tcp.host, node_addr->addr.tcp.service,
                    DEF_PROTOCOL);
  if (fd < 0) {
    logger.error("Could not bind to socket %s:%s\n", (node_addr->addr.tcp.host),
                 node_addr->addr.tcp.service);
    goto error_exit_0;
  }
  if (listen(fd, DEF_MAX_PENDING_CONNECTION_REQUESTS) < 0)
    goto error_exit_0;

  /* find a free node descriptor */
  if ((nd = nd_table_get_free_node(&nd_table_, MB_LISTEN_NODE)) < 0) {
    /* if no free nodes to initialize, then we are finished... */
    goto error_exit_1;
  }

  /* nd_table_.node[nd].addr = tmp_addr; */ /* does not apply for MB_LISTEN_NODE
                                             */
  nd_table_.node[nd].fd = fd;               /* not currently connected... */

  logger.debug("[%lu] modbus_tcp_listen(): returning nd=%d fd=%d\n",
               Rtos::pthread_self(), nd, fd);

  return &nd_table_.node[nd];

error_exit_1:
  closesocket(fd);
error_exit_0:
  return nullptr;
}

/******************************/
/**                          **/
/**       Close a node       **/
/**                          **/
/******************************/

int modbus_tcp_close(int nd) {

  logger.debug("[%lu] modbus_tcp_close(): called... nd=%d\n",
               Rtos::pthread_self(), nd);

  if ((nd < 0) || (nd >= nd_table_.node_count)) {
    /* invalid nd */

    logger.error("[%lu] modbus_tcp_close(): invalid node %d. Should be < %d\n",
                 Rtos::pthread_self(), nd, nd_table_.node_count);

    return -1;
  }

  if (nd_table_.node[nd].node_type == MB_FREE_NODE)
    /* already free node */
    return 0;

  close_connection(&nd_table_.node[nd]);

  nd_table_close_node(&nd_table_, &nd_table_.node[nd]);

  return 0;
}

/**********************************/
/**                              **/
/**  Close all open connections  **/
/**                              **/
/**********************************/

int modbus_tcp_silence_init() {
  //    logger.debug("[%lu] modbus_tcp_silence_init(): called...\n",
  //    Rtos::pthread_self());

  /* close all master connections that remain open... */
  for (int nd = 0; nd < nd_table_.node_count; nd++)
    if ((nd_table_.node[nd].node_type == MB_MASTER_NODE) &&
        (nd_table_.node[nd].close_on_silence > 0))
      /* node is is being used for a master device,
       * and wishes to be closed...   ...so we close it!
       */
      close_connection(&nd_table_.node[nd]);

  return 0;
}

/******************************/
/**                          **/
/**   Shutdown the Library   **/
/**                          **/
/******************************/

int modbus_tcp_done() {
  modbus_tcp_init_counter--;
  if (modbus_tcp_init_counter != 0)
    return 0; /* ignore this request */

  /* close all the connections... */
  for (int i = 0; i < nd_table_.node_count; i++)
    modbus_tcp_close(i);

  /* Free memory... */
  nd_table_done(&nd_table_);

  return 0;
}

double modbus_tcp_get_min_timeout(uint32_t baud, u8 parity, u8 data_bits,
                                  u8 stop_bits) {
  return 0;
}

int my_select_tcp(int fd, fd_set *rfds, fd_set *wfds,
                  struct timespec *end_time) {

  int res;
  struct timeval timeout {};
  struct timeval *tv_ptr;
  fd_set tmp_rfds;
  fd_set *tmp_rfds_ptr;
  fd_set tmp_wfds;
  fd_set *tmp_wfds_ptr;

  tmp_rfds_ptr = nullptr;
  tmp_wfds_ptr = nullptr;
  if (rfds != nullptr)
    tmp_rfds_ptr = &tmp_rfds;
  if (wfds != nullptr)
    tmp_wfds_ptr = &tmp_wfds;

  /*============================*
   * wait for data availability *
   *============================*/
  do {
    if (rfds != nullptr)
      tmp_rfds = *rfds;
    if (wfds != nullptr)
      tmp_wfds = *wfds;
    /* NOTE: To do the timeout correctly we would have to revert to timers
     *       and asociated signals. That is not very thread friendly, and is
     *       probably too much of a hassle trying to figure out which signal
     *       to use. What if we don't have any free signals?
     *
     *       The following solution is not correct, as it includes a race
     *       condition. The following five lines of code should really
     *       be atomic!
     *
     * NOTE: see also the timeout related comment in the
     *       modbus_tcp_read() function!
     */
    if (end_time == nullptr) {
      tv_ptr = nullptr;
    } else {
      tv_ptr = &timeout;
      if ((end_time->tv_sec == 0) && (end_time->tv_nsec == 0)) {
        timeout.tv_sec = timeout.tv_usec = 0;
      } else {
        timeout = timespec_to_timeval(end_time);
      }
    }

    res = select(fd, tmp_rfds_ptr, tmp_wfds_ptr, nullptr, tv_ptr);

    {
      int i;
      // if (tmp_rfds_ptr != nullptr)
      //     for (i = 0; i < fd; i++)
      //         if (FD_ISSET(i, tmp_rfds_ptr))
      //         {
      //             //                        logger.debug("fd=%d is ready for
      //             reading\n", i);
      //         }
      // if (tmp_wfds_ptr != nullptr)
      // for (i = 0; i < fd; i++)
      //     if (FD_ISSET(i, tmp_wfds_ptr))
      //     {
      //         //                        logger.debug("fd=%d is ready for
      //         writing\n", i);
      //     }
    }

    if (res == 0) {

      return 0;
    }
    if ((res < 0) && (errno != EINTR)) {
      return -1;
    }
  } while (res <= 0);

  if (rfds != nullptr)
    *rfds = tmp_rfds;
  if (wfds != nullptr)
    *wfds = tmp_wfds;
  return res;
}

#endif
